#include "motor_task.h"
#include "pid_ctrl.h"
#include "svpwm.h"
#include "smo_cal.h"
#include "fast_math.h"
#include "string.h"
#include "stdio.h"
#include "pid_ctrl.h"
extern VOFA_SHOW_DATA_T VOFA_SHOW_DATA;
motor_go_t motor_go;
motor_smo_t motor_infot;
motor_iabc_t motor_iabc;
motor_duty_abc_t motor_duty_abc;
motor_uab_t motor_uab;
motor_iab_t motor_iab;
motor_idq_ref_t motor_idq_ref;
motor_idq_t motor_idq;
opl_inf_t opl_inf;
float theta_foc_t = 0 ;
float speed_foc = 0;
float PWM_frequency 	= 20e3;
float T_pwm           = 1/20e3;
float Ts          	  = 1/20e3;
float target_spd = 1200;
pid_struct_t _s_pid,_cd_pid,_cq_pid;

void limit_ang(float *ang){
    if(*ang<0) *ang+=2*PI;
    else if (*ang>2*PI) *ang-=2*PI;
    else *ang = *ang;
}

void init_pra(opl_inf_t* init_data){
	
    init_data->OPL_Iq = 6.f;
    init_data->OPL_Speed = 3000.f ;
    init_data->OPL_Time = 0.3f;
    init_data-> pmsm_p                 =7;
    init_data-> pmsm_Rs                =0.194F;
    init_data-> pmsm_Ld                =0.097e-03;
    init_data-> pmsm_Lq                =0.097e-03;
    init_data-> pmsm_Ls                =init_data-> pmsm_Ld;
    init_data-> pmsm_J                 =2.2254e-8;
    init_data-> pmsm_B                 =2.6715e-8;
    init_data-> pmsm_N_max             =15000*6;
    init_data-> pmsm_Ke                =0.3;
    init_data-> pmsm_FluxPM            =(init_data-> pmsm_Ke)/(1.732f*2*PI*1000*init_data-> pmsm_p/60);
    init_data-> pmsm_Kt                =(3/2)*init_data-> pmsm_FluxPM*init_data-> pmsm_p;
    init_data-> pmsm_I_rated           =1.2F;
    init_data-> pmsm_N_base            =12540*2;
    init_data-> pmsm_F_base            =init_data-> pmsm_N_base*init_data-> pmsm_p/60;
    init_data-> pmsm_DC                =24.F;
    init_data-> pmsm_T_rated           = (3/2)*init_data-> pmsm_p*init_data-> pmsm_FluxPM*init_data-> pmsm_I_rated;

    memcpy(&_s_pid,&_s_PID,sizeof(pid_struct_t));
    memcpy(&_cd_pid,&_cd_PID,sizeof(pid_struct_t));
    memcpy(&_cq_pid,&_cq_PID,sizeof(pid_struct_t));

}

void m_task_10ms(void){
    motor_go.count++;
    if (motor_go.count < 50){
        motor_go.motor_state = step_0;
    }
    else if (motor_go.count > 50 && motor_go.count< 100 ){
        motor_go.motor_state = step_1;
    }
    else if (motor_go.count >100 &&motor_infot._motor_spd>900*7){
        motor_go.motor_state = step_2;
    }
}

void speed_control(float target_speed,motor_smo_t* smo_inf,motor_go_t* mstate,motor_idq_ref_t* idq_ref,
                                                                                        pid_struct_t* pid_f){//500hz
    if(mstate->motor_state == step_2){
        idq_ref->_iq_ref = pid_calc(pid_f,target_speed,smo_inf->_motor_spd);
        idq_ref->_id_ref = 0;
    }
}

		static float acc = 0;
		static float speed_temp = 0;
void Schedule_tran(motor_smo_t* smo_inf,motor_go_t* mstate,float* theta_foc,float *speed_foc,
                                                                           motor_idq_ref_t* idq_ref){

    switch (mstate->motor_state){
        case step_0 :
            idq_ref->_id_ref =opl_inf.OPL_Iq ;
            idq_ref->_iq_ref = 0;
            *theta_foc = 0.f;
            break;
        case step_1:
            idq_ref->_id_ref = opl_inf.OPL_Iq;
            idq_ref->_iq_ref =  0; 
            acc = opl_inf.OPL_Speed/opl_inf.OPL_Time;
            speed_temp = acc*Ts*opl_inf.pmsm_p/60.f*2*PI;
            *speed_foc += speed_temp;
            *theta_foc += *speed_foc *Ts;
            //limit_ang(theta_foc);
            break;
        case step_2:
            idq_ref->_id_ref = idq_ref->_id_ref;
            idq_ref->_iq_ref = idq_ref->_iq_ref;
            *theta_foc = smo_inf->_motor_ang;
            //limit_ang(theta_foc);
            break;
        default:
            printf("err");
            break;
    }
}

void cal_speed_ob(motor_uab_t uab,motor_iab_t iab,motor_smo_t* smo_d){
    smo_cal_step(uab._ual,uab._ube,iab._ial,iab._ibe,&smo_d->_motor_ang,&smo_d->_motor_spd);
}

void SMO_FOC(motor_idq_ref_t* idq_ref,motor_iabc_t* iabc,motor_duty_abc_t* duty_abc,
                                                                    motor_iab_t* ialbe,
                                                                    motor_idq_t* idq,
                                                                    pid_struct_t* pid_uq,
                                                                    pid_struct_t* pid_ud){
    calrk_func(iabc,ialbe);
    park_func(ialbe,theta_foc_t,idq);
    float ud = pid_calc(pid_ud,idq_ref->_id_ref,idq->_id);
    float uq = pid_calc(pid_uq,idq_ref->_iq_ref,idq->_iq);
    svpwm_func(ud,uq,theta_foc_t,24.f,1000-1,&svpwm,&motor_uab);//have done the invpark inside fx
    duty_abc->_dutya = svpwm.TEMP1_p;
    duty_abc->_dutyb = svpwm.TEMP2_P;
    duty_abc->_dutyc = svpwm.TEMP3_P;
}

void m_task_2ms(void){
    speed_control(target_spd,&motor_infot,&motor_go,&motor_idq_ref,&_s_pid);
}

void _20khz_task(void){
    Schedule_tran(&motor_infot,&motor_go,&theta_foc_t,&speed_foc,&motor_idq_ref);
    cal_speed_ob(motor_uab,motor_iab,&motor_infot);
    SMO_FOC(&motor_idq_ref,&motor_iabc,&motor_duty_abc,&motor_iab,&motor_idq,&_cq_pid,&_cd_pid);
}

